#include "CollisionDetection.h"
#include "Phycccp.h"
#include "../shared/shared_types.h"
#include "../shared/Time.h"

namespace Putin{

Node::Node(Wrapper *wrapper, bool begin)
{
    mWrapper = wrapper;
    mBegin = begin;
}

Wrapper::Wrapper(Geometry *geo)
{
    mGeometry = geo;	
    mXbegin = new Node(this, true);
    mXend = new Node(this, false);
    mYbegin = new Node(this, true);
    mYend = new Node(this, false);
}

Wrapper::~Wrapper()	
{
	delete(mXbegin);
	delete(mYbegin);
	delete(mYend);
	delete(mXend);
}

void Wrapper::Update()    
{
    mColliders.clear();
	mXbegin->mValue = mGeometry->mAABB.P.x-mGeometry->mAABB.E.x;
	mXend->mValue = mGeometry->mAABB.P.x+mGeometry->mAABB.E.x;
    mYbegin->mValue = mGeometry->mAABB.P.y-mGeometry->mAABB.E.y;
    mYend->mValue = mGeometry->mAABB.P.y+mGeometry->mAABB.E.y;
}

bool SortNodes::operator()(Node* left, Node* right) const
{
    //return true if left is logically less then right for given comparison
    if(left->mValue < right->mValue)
            return true;
    else
            return false;
}

bool RemoveDisposedWrapper::operator()(Wrapper* w) const
{   
	if(w->mGeometry->mIsDesposed)
	{
		delete(w);
		return 1;  
	}
	return 0;
}
bool RemoveDisposedNode::operator()(Node* n) const
{   
	return n->mWrapper->mGeometry->mIsDesposed;  
}
void HandleFirstCollision::execute(Wrapper* w1, Wrapper* w2, CollisionDetection* coldet)
{
	/*
	 * if(early fail conditions) {return;}
	 * This could be explosion vs explosion but how often is that gonna happen?
	 */
	w1->mColliders.push_back(w2->mGeometry);
    w2->mColliders.push_back(w1->mGeometry);
}
void HandleSecondCollision::execute(Wrapper* w1, Wrapper* w2, CollisionDetection* coldet){
	for each(Geometry *geometries in w1->mColliders)
	{
		if(geometries == w2->mGeometry)
		{
			/* this is a confirmed broadphase collision
			 * should check what kind of collision it is and
			 * call the proper detection routine.
			 */
			if((w1->mGeometry->mPhysicsObject->mType == DynamicType)||(w2->mGeometry->mPhysicsObject->mType == DynamicType)){
				//Atleast one of the objects are dynamic, the pair is added to the map
				coldet->mCollidingObjects.insert(std::make_pair(w1->mGeometry->mPhysicsObject,w2->mGeometry->mPhysicsObject)); 
			}

		}
	}	
}

CollisionDetection::~CollisionDetection(){}//Add code to kill all wrappers and nodes on deletion

void CollisionDetection::AddGeometry(Geometry* item)
{
    Wrapper* wrapper = new Wrapper(item);
    mWrappers.push_back(wrapper);
    mXlist.push_back(wrapper->mXbegin);
    mXlist.push_back(wrapper->mXend);
    mYlist.push_back(wrapper->mYbegin);
    mYlist.push_back(wrapper->mYend);		
}
void CollisionDetection::RemoveDisposed()
{
	//Removes objects from the list, start with the nodes since RemoveDisposedWrapper will also delete the wrapper
	mXlist.remove_if(RemoveDisposedNode());
	mYlist.remove_if(RemoveDisposedNode());
	mWrappers.remove_if(RemoveDisposedWrapper());
}
void CollisionDetection::Run(bool collisionson)
{
	mCollisionsOn = collisionson;
    Update();
	mCollidingObjects.clear();
    RunAxis(mXlist, new HandleFirstCollision);
    RunAxis(mYlist, new HandleSecondCollision);
	//Do something about the objects in our colliders list
	RunList();
}
void CollisionDetection::Update()
{
	//Update wrappers with positions set to geometries
	for each (Wrapper *wrapper in mWrappers)
    {
        wrapper->Update();
    }
	//Sort nodes
    mXlist.sort(SortNodes());
	mYlist.sort(SortNodes());
}
void CollisionDetection::RunAxis(std::list<Node*> list, CollisionFunction* collisionFunction)
{
	std::list<Wrapper*> proximityList;

	for each (Node *node in list)
    {
        if (node->mBegin)
        {
            for each (Wrapper *wrapper in proximityList)
            {
                collisionFunction->execute(node->mWrapper, wrapper, this);
            }
			proximityList.push_back(node->mWrapper);
        }
        else
        {
			proximityList.pop_back();
        }
    }
	delete(collisionFunction);
}
void CollisionDetection::RunList()
{
	
	for each(std::pair<PhysicsObject*, PhysicsObject*> c in mCollidingObjects)
	{
		//Check if its a projectile
		if(c.first->mType == ProjectileType)
			CollideDynamicVsProjectile(c.second,c.first);
		else if(c.second->mType == ProjectileType)
			CollideDynamicVsProjectile(c.first, c.second);
		//Check if two dynamic objects are colliding
		else if(mCollisionsOn)
		{
			if((c.first->mType == DynamicType)&&(c.second->mType == DynamicType))
				CollideDynamicVsDynamic(c.first, c.second);
			//Check if a static and a dynamic object are colliding
			else if(c.first->mType == StaticType)
				CollideDynamicVsStatic(c.second,c.first);
			else if(c.second->mType == StaticType)
				CollideDynamicVsStatic(c.first,c.second);
		}
	}
}

void CollisionDetection::CollideDynamicVsProjectile(PhysicsObject* po1, PhysicsObject* po2)
{
	ProjectilePhysicsObject * proj = (ProjectilePhysicsObject*) po2;
	DynamicPhysicsObject * dyn = (DynamicPhysicsObject*) po1;

	float d = sqrt((dyn->mGeometry->mAABB.P-proj->mTarget)*(dyn->mGeometry->mAABB.P-proj->mTarget)); 
	if(d < proj->mPoc->splashRange)
	{
		proj->mBullet->mUnitsHit.push_back(dyn->mUnit->getID());
		proj->mBullet->mDistFromHit.push_back(d);
		proj->mBullet->mHasExploded = true;
	}

}

void CollisionDetection::CollideDynamicVsDynamic(PhysicsObject* po1, PhysicsObject* po2)
{
	DynamicPhysicsObject * dyn1 = (DynamicPhysicsObject*) po1;
	DynamicPhysicsObject * dyn2 = (DynamicPhysicsObject*) po2;

	//if (dyn1->mGeometry->mOBB.P !=  dyn1->mPosition)
		dyn1->mGeometry->UpdateOBB(dyn1->mNormal,dyn1->mDirection);
	//if (dyn2->mGeometry->mOBB.P !=  dyn2->mPosition)
		dyn2->mGeometry->UpdateOBB(dyn2->mNormal,dyn2->mDirection);
	
	
	if(1==1)
	if(Intersect(dyn1->mGeometry->mOBB,dyn2->mGeometry->mOBB))
	{
		float k = 0.00000001;
		float c = dyn2->mConstants->weight/dyn1->mConstants->weight;
		Vec3f x1 = ((dyn1->mSpeed)+(dyn2->mSpeed)*c)*(-2/(1+c));
		Vec3f x2;
		x2.x = dyn1->mSpeed.x*dyn1->mSpeed.x*k + dyn2->mSpeed.x*dyn2->mSpeed.x*k*c;
		x2.y = dyn1->mSpeed.y*dyn1->mSpeed.y*k + dyn2->mSpeed.y*dyn2->mSpeed.y*k*c;
		x2.z = 0;
		
		if ((x2.x)<0)
			x2.x = -x2.x;
	
		if ((x2.y)<0)
			x2.y = -x2.y;
		
		
		
		x1.x = x1.x*(-1/2)+sqrt(x2.x);
		x1.y = x1.y*(-1/2)+sqrt(x2.y);
		x1.z = x1.z*(-1/2)+sqrt(x2.z);

		x2 = dyn1->mSpeed + dyn2->mSpeed*c -x1*c;

		// Uncomment to prevent box-assertion crash..
		dyn1->mSpeed = x2;
		dyn2->mSpeed = x1;
		float h = Time::instance().getLastFrameTime();
		dyn1->mPosition = dyn1->mPositionOld + dyn1->mSpeed*h;
		dyn2->mPosition = dyn2->mPositionOld + dyn2->mSpeed*h;
	
	}

}

void CollisionDetection::CollideDynamicVsStatic(PhysicsObject* po1, PhysicsObject* po2)
{
	DynamicPhysicsObject * dyn = (DynamicPhysicsObject*) po1;
	StaticPhysicsObject * stat = (StaticPhysicsObject*) po2;

	if (dyn->mGeometry->mOBB.P !=  dyn->mPosition)
		dyn->mGeometry->UpdateOBB(dyn->mNormal,dyn->mDirection);

	if(Intersect(dyn->mGeometry->mOBB,stat->mGeometry->mOBB))
	{
		dyn->mSpeed = Vec3f(0,0,0);
		dyn->mPosition = dyn->mPositionOld;
	}

}
//----------------------------------------------------------------------------------------------//
//								OOBB Collision detection										//
//							(Narrow phase collision detection)									//
//----------------------------------------------------------------------------------------------//

bool CollisionDetection::Overlap(float min0, float max0, float min1, float max1)
{
	return (min0 > max1 || max0 < min1);
}

bool CollisionDetection::SpanOverlap(float min0, float max0, float min1, float max1)
{
	return !(min0 > max1 || max0 < min1);
}
void CollisionDetection::ComputeSpan(Vec3f Axis, OrientedBoundingBox Box, float& min, float& max)
{
	float p = Axis * Box.P;
	float r = fabs(Axis * Box.X) * Box.E.x + fabs(Axis * Box.Y) * Box.E.y + fabs(Axis * Box.Z) * Box.E.z;

	min = p-r;
	max = p+r;
}
bool CollisionDetection::AxisOverlap(Vec3f Axis, OrientedBoundingBox B0, OrientedBoundingBox B1)
{
	float min0, max0;
	float min1, max1;
	ComputeSpan(Axis, B0, min0, max0);
	ComputeSpan(Axis, B1, min1, max1);

	return SpanOverlap(min0, max0, min1, max1);
}

bool CollisionDetection::Intersect(OrientedBoundingBox B0, OrientedBoundingBox B1)
{
	if (!AxisOverlap(B0.X, B0, B1))
		 return false;
	if (!AxisOverlap(B0.Y, B0, B1))
		 return false;
	if (!AxisOverlap(B0.Z, B0, B1))
		 return false;

	if (!AxisOverlap(B0.X^B1.X, B0, B1))
		 return false;
	if (!AxisOverlap(B0.X^B1.Y, B0, B1))
		 return false;
	if (!AxisOverlap(B0.X^B1.Z, B0, B1))
		 return false;

	if (!AxisOverlap(B0.Y^B1.X, B0, B1))
		 return false;
	if (!AxisOverlap(B0.Y^B1.Y, B0, B1))
		 return false;
	if (!AxisOverlap(B0.Y^B1.Z, B0, B1))
		 return false;

	if (!AxisOverlap(B0.Z^B1.X, B0, B1))
		 return false;
	if (!AxisOverlap(B0.Z^B1.Y, B0, B1))
		 return false;
	if (!AxisOverlap(B0.Z^B1.Z, B0, B1))
		 return false;
   
	return true;
}

}// End namespace putin